home *** CD-ROM | disk | FTP | other *** search
/ Atari Mega Archive 1 / Atari Mega Archive - Volume 1.iso / graphics / qrt.lzh / MTH.C < prev    next >
C/C++ Source or Header  |  1989-02-16  |  8KB  |  406 lines
  1. /**********************************************************
  2.  
  3.                   File for vector math
  4.  
  5.  **********************************************************/
  6.  
  7. #include <math.h>
  8. #include "qrt.h"
  9.  
  10. /* #define MATHDEBUG TRUE */
  11. /* #define MATHMAIN TRUE */
  12.  
  13.  
  14. /**********************************************************
  15.  
  16.     Returns pseudo random number between one and 100
  17.  
  18.  **********************************************************/
  19.  
  20. int PsRand() {
  21.   static int val=43;
  22.  
  23.   val=(val*73+97);
  24.   return(val%CNUM);
  25. }
  26.  
  27. /**********************************************************
  28.                                     -
  29.                     Vector clear to 0
  30.  
  31.  **********************************************************/
  32.  
  33. VectEqZero(v)
  34.   VECT_PTR v;
  35. {
  36.   v->x=v->y=v->z=0.0;
  37. }
  38.  
  39.  
  40. /**********************************************************
  41.                    -    -       -       -
  42.      Vector math:  V1 = V1 + k2 V2 + k3 V3
  43.  
  44.  **********************************************************/
  45.  
  46. VectAddMult(v1,k2,v2,k3,v3)
  47.   VECT_PTR v1,v2,v3;
  48.   float k2,k3;
  49. {
  50.   v1->x+=k2*(v2->x)+k3*(v3->x);
  51.   v1->y+=k2*(v2->y)+k3*(v3->y);
  52.   v1->z+=k2*(v2->z)+k3*(v3->z);
  53. }
  54.  
  55. /**********************************************************
  56.                    -      -
  57.      Vector math:  V1 = - V2
  58.  
  59.  **********************************************************/
  60.  
  61. VectNegate(v1,v2)
  62.   VECT_PTR v1,v2;
  63. {
  64.   v1->x = -(v2->x);
  65.   v1->y = -(v2->y);
  66.   v1->z = -(v2->z);
  67. }
  68.  
  69.  
  70. /**********************************************************
  71.                    -    -    -
  72.      Vector math:  V1 = V2 + V3
  73.  
  74.  **********************************************************/
  75.  
  76. VectorAdd(v1,v2,v3)
  77.   VECT_PTR v1,v2,v3;
  78. {
  79.   v1->x=v2->x+v3->x;
  80.   v1->y=v2->y+v3->y;
  81.   v1->z=v2->z+v3->z;
  82. }
  83.  
  84.  
  85. /**********************************************************
  86.                    -    -    -
  87.      Vector math:  V1 = V2 * V3
  88.  
  89.  **********************************************************/
  90.  
  91. VectorMult(v1,v2,v3)
  92.   VECT_PTR v1,v2,v3;
  93. {
  94.   v1->x=v2->x*v3->x;
  95.   v1->y=v2->y*v3->y;
  96.   v1->z=v2->z*v3->z;
  97. }
  98.  
  99.  
  100. /**********************************************************
  101.                    -    -    -
  102.      Vector math:  V1 = V2 - V3
  103.  
  104.  **********************************************************/
  105.  
  106. VecSubtract(v1,v2,v3)                              /* 1=2-3 */
  107.   VECT_PTR v1,v2,v3;
  108. {
  109.   v1->x=v2->x-v3->x;
  110.   v1->y=v2->y-v3->y;
  111.   v1->z=v2->z-v3->z;
  112. }
  113.  
  114.  
  115. /**********************************************************
  116.                    -      -
  117.      Vector math:  V1 = k V1
  118.  
  119.  **********************************************************/
  120.  
  121. VectScale(v1,k,v2)                               /* 1=2*1 */
  122.   VECT_PTR v1;
  123.   float k;
  124. {
  125.   v1->x *= k;
  126.   v1->y *= k;
  127.   v1->z *= k;
  128. }
  129.  
  130.  
  131. /**********************************************************
  132.  
  133.                 Color vector assignment
  134.  
  135.  **********************************************************/
  136.  
  137. SVectEQ(v1,v2)                    /* short vector v1=v2 */
  138.   SVECT_PTR v1,v2;
  139. {
  140.   v1->r=v2->r;
  141.   v1->g=v2->g;
  142.   v1->b=v2->b;
  143. }
  144.  
  145.  
  146. /**********************************************************
  147.  
  148.                     Vector assignment
  149.  
  150.  **********************************************************/
  151.  
  152. VectEQ(v1,v2)                     /* long vector v1=v2 */
  153.   VECT_PTR v1,v2;
  154. {
  155.   v1->x=v2->x;
  156.   v1->y=v2->y;
  157.   v1->z=v2->z;
  158. }
  159.  
  160.  
  161. /**********************************************************
  162.  
  163.                   Normalize vector V
  164.  
  165.  **********************************************************/
  166.  
  167. Normalize(v)
  168.   VECT_PTR v;
  169. {
  170.   register float l;
  171.   l=sqrt(DotProd((*v),(*v)));
  172.  
  173.   v->x/=l;
  174.   v->y/=l;
  175.   v->z/=l;
  176. }
  177.  
  178.  
  179. /**********************************************************
  180.                    -    -    -
  181.      Vector math:  V1 = V2 x V3
  182.  
  183.  **********************************************************/
  184.  
  185. CrossProd(v1,v2,v3)
  186.   VECT_PTR v1,v2,v3;
  187. {
  188.   v1->x=v2->y*v3->z-v2->z*v3->y;
  189.   v1->y=v2->z*v3->x-v2->x*v3->z;
  190.   v1->z=v2->x*v3->y-v2->y*v3->x;
  191. }
  192.  
  193.  
  194. /**********************************************************
  195.  
  196.       Returns position vector given line structure
  197.       and parameter T
  198.  
  199.  **********************************************************/
  200.  
  201. FindPos(pos,line2,t)      /* find position on parametric line 2 */
  202.   VECT_PTR pos;
  203.   OBJ_PTR  line2;
  204.   float t;
  205. {
  206. # ifdef ROBUST
  207.     if (line2->type!=LINE) Error(INTERNAL_ERROR,401);
  208. # endif
  209.  
  210.   pos->x=line2->loc.x+line2->vect1.x*t;
  211.   pos->y=line2->loc.y+line2->vect1.y*t;
  212.   pos->z=line2->loc.z+line2->vect1.z*t;
  213.  
  214. # ifdef MATHDEBUG
  215.     printf("FINDPOS: t=%f, Xout=%f, Yout=%f, Zout=%f\n",
  216.             t,pos->x,pos->y,pos->z);
  217. # endif
  218. }
  219.  
  220. /**********************************************************
  221.  
  222.         Reflects vector IN about NORM to produce OUT
  223.  
  224.  **********************************************************/
  225.  
  226. Reflect(out,in,norm)
  227.   VECT_PTR out,in,norm;
  228. {
  229.   float dot;
  230.  
  231.   dot = -DotProd((*in),(*norm));
  232.  
  233. # ifdef ROBUST                           /* CHECK THIS OUT */
  234.     /* if (dot<0) Error (INTERNAL_ERROR,402); */
  235. # endif
  236.  
  237.   VectEQ(out,in);
  238.   VectAddMult(out,dot,norm,dot,norm);
  239.  
  240. # ifdef MATHDEBUG
  241.     printf("REFLECT: in   x,y,z = %f %f %f\n",in->x,in->y,in->z);
  242.     printf("         norm x,y,z = %f %f %f\n",norm->x,norm->y,norm->z);
  243.     printf("         out  x,y,z = %f %f %f\n",out->x,out->y,out->z);
  244. # endif
  245. }
  246.  
  247.  
  248. /**********************************************************
  249.  
  250.    Arctan of 2 numbers.  This *should* exist in the
  251.    compiler library, but it's not there.
  252.  
  253.  **********************************************************/
  254.  
  255. float atan2w(x,y)
  256.   float x,y;
  257. {
  258.   if (x==0) {
  259.     if (y>0) return(PI2);
  260.     return(-PI2);
  261.   }
  262.  
  263.   if (x>0) return(atan(y/x));
  264.  
  265.   if (y>0) return(PI-atan(y/(-x)));
  266.  
  267.   return(-PI+atan(y/x));
  268. }
  269.  
  270.  
  271. /**********************************************************
  272.  
  273.                     DOUBLE ROTATION
  274.  
  275.    This is a really two multiplied transformation
  276.    matricies, but this form is faster. The if statements
  277.    rotate only if necessary.
  278.  
  279.  **********************************************************/
  280.  
  281. Rot12(invect,outvect,cos1,sin1,cos2,sin2)
  282.   VECT_PTR invect, outvect;
  283.   register float cos1, sin1, cos2, sin2;
  284. {
  285. #ifdef ROTATEFINISH
  286.  
  287.   VECTOR tempdir;
  288.  
  289.   /** Rotate once to YZ plane **/
  290.  
  291.   if ((cos1!=0) || (sin1!=0)) {
  292.  
  293.     tempdir.x = invect->x *  cos1 +
  294.                 invect->z * -sin1;
  295.  
  296.     tempdir.y = invect->y;
  297.  
  298.     tempdir.z = invect->x *  sin1 +
  299.                 invect->z *  cos1;
  300.   } else {
  301.     VectEQ(&tempdir,invect);
  302.   }
  303.  
  304.   /** Rotate again to positive Y **/
  305.  
  306.   if ((cos2!=0) || (sin2!=0)) {
  307.  
  308.     outvect->x = tempdir.x;
  309.  
  310.     outvect->y = tempdir.y * -cos2 +
  311.                  tempdir.z *  sin2;
  312.  
  313.  
  314.     outvect->z = tempdir.y * -sin2 +
  315.                  tempdir.z * -cos2;
  316.  
  317.   } else {
  318.     VectEQ(outvect,&tempdir);
  319.   }
  320. #endif
  321. }
  322.  
  323.  
  324. /**********************************************************
  325.  
  326.                     DOUBLE ROTATION
  327.  
  328.        Same as above, but in reverse direction
  329.  
  330.  **********************************************************/
  331.  
  332. Rot21(invect,outvect,cos1,sin1,cos2,sin2)
  333.   VECT_PTR invect, outvect;
  334.   float cos1, sin1, cos2, sin2;
  335. {
  336. #ifdef ROTATEFINISH
  337.  
  338.   VECTOR tempdir;
  339.  
  340.   /** Rotate once from Y up direction **/
  341.  
  342.   if ((cos2!=0) || (sin2!=0)) {
  343.  
  344.     tempdir.x = invect->x;
  345.  
  346.     tempdir.y = invect->y *  cos2 +
  347.                 invect->z * -sin2;
  348.  
  349.  
  350.     tempdir.z = invect->y *  sin2 +
  351.                 invect->z *  cos2;
  352.  
  353.   } else {
  354.     VectEQ(&tempdir,invect);
  355.   }
  356.  
  357.   /** Rotate again to arbitrary direction **/
  358.  
  359.   if ((cos1!=0) || (sin1!=0)) {
  360.  
  361.     outvect->x = tempdir.x * -cos1 +
  362.                  tempdir.z *  sin1;
  363.  
  364.     outvect->y = tempdir.y;
  365.  
  366.     outvect->z = tempdir.x * -sin1 +
  367.                  tempdir.z * -cos1;
  368.   } else {
  369.     VectEQ(outvect,&tempdir);
  370.   }
  371. #endif
  372. }
  373.  
  374.  
  375. /**********************************************************
  376.  
  377.       Main for mth.c - used ONLY to test functions.
  378.  
  379.  **********************************************************/
  380.  
  381. # ifdef MATHMAIN
  382.  
  383. Error() { }
  384.  
  385. main() {
  386.  
  387.   VECTOR in,out;
  388.   float sin1, sin2, cos1, cos2;
  389.  
  390.   printf("in: ");
  391.   scanf("%f %f %f",&(in.x),&(in.y),&(in.z));
  392.  
  393.   printf("sin1 cos1 sin2 cos2: ");
  394.   scanf("%f %f %f %f",&sin1, &cos1, &sin2, &cos2);
  395.  
  396.   Rot12(&in,&out,cos1,sin1,cos2,sin2);
  397.  
  398.   printf("out12: %f %f %f\n",out.x,out.y,out.z);
  399.  
  400.   Rot21(&in,&out,cos1,sin1,cos2,sin2);
  401.   printf("out21: %f %f %f\n",out.x,out.y,out.z);
  402. }
  403.  
  404. # en